/**
 * @file ROS_map_plan.cpp
 * @author circleup (circleup@foxmail.com)
 * @brief GPS 位置相关计算服务函数
 * @version 2.7
 * @date 2020-07-08
 * 
 * @copyright Copyright (c) 2020
 * 
 */


#include "ros_serial_driver.h"
#include "ros_control_driver.h"
#include "ROS_map_plan.h"

#include <visualization_msgs/Marker.h>

#include<iostream>
#include<fstream>

#include "ros/ros.h"
#include "ros_car/map_plan.h"

/**
 * @brief 规划点容器
 * 
 */
std::vector<POS_T> g_points_plan;

/**
 * @brief 当前点容器
 * 
 */
std::vector<POS_T> g_points_current;


/**
 * @brief rviz 显示规划点容器
 * 
 */
visualization_msgs::Marker g_rviz_points_plan;

/**
 * @brief rviz 显示当前点容器
 * 
 */
visualization_msgs::Marker g_rviz_point_current;

/**
 * @brief map_plan 服务函数，返回规划点与当前点的角度和状态
 * 
 * @param req 当前点 GPS 坐标
 * @param res 角度和求解状态
 */
bool ROSMapPlanCallback(ros_car::map_plan::Request  &req, ros_car::map_plan::Response &res);


int main(int argc, char **argv)
{
  ros::init(argc, argv, "ROS_map_plan_server");
  ros::NodeHandle n;

  ros::ServiceServer service = n.advertiseService("ros_map_plan", ROSMapPlanCallback);
  ROS_INFO("This is ROS_map_plan_server.");

  ros::Publisher marker_pub = n.advertise<visualization_msgs::Marker>("visualization_marker", 100);

  init_rviz_points();

  AddPlanPoint();
  ROS_INFO(" Loaded piont size is %d", g_points_plan.size());
  
  ros::Rate loop_rate(30);

  while (ros::ok())
  {
    update_rviz_points();
    marker_pub.publish(g_rviz_point_current);
    marker_pub.publish(g_rviz_points_plan);
    
    loop_rate.sleep();
    ros::spinOnce();
  }

  return 0;
}


bool ROSMapPlanCallback(ros_car::map_plan::Request  &req, ros_car::map_plan::Response &res)
{
  PLANPOINTSTATUS_T plan_points_status;
  POS_T pos;

  pos.lattitude = req.lattitude;
  pos.longitude = req.longitude;

  g_points_current.push_back(pos);

  plan_points_status = FindTargetPoint(pos);

  if (Normal == plan_points_status)
  {
    // ROS_INFO("req.lattitude is %s, req.longitude is %s", DoubleToString(req.lattitude, 8).c_str(), DoubleToString(req.longitude, 8).c_str());
    // ROS_INFO("plan_point.lattitude is %s, plan_point.longitude is %s", DoubleToString(g_points_plan[0].lattitude, 8).c_str(), DoubleToString( g_points_plan[0].longitude, 8).c_str());

    res.angle = GPSAngleCalculateUTMFromGPS(req.longitude, req.lattitude, g_points_plan[0].longitude, g_points_plan[0].lattitude);
    res.status = true;

    if((0 == req.lattitude) && (0 == req.longitude))
    {
      res.angle = 0;
      res.status = false;
    }
  }
  else
  {
    res.angle = 0;
    res.status = false;
  }
  return true;
}

bool AddPlanPoint()
{
  POS_T data;
  std::vector<std::string> v_temp;
  std::string temp;
  std::ifstream fin(FILEPATH);

  if(!fin)
  {
    ROS_INFO("OPEN FILE FAILED");
    return false;
  }
  else
  {
    while (getline(fin, temp))
    {
      SplitString(temp, v_temp, ",");
      data.lattitude = StringToDouble(v_temp[1].c_str());
      data.longitude = StringToDouble(v_temp[2].c_str());
      g_points_plan.push_back(data);

      temp.clear();
      v_temp.clear();
      data.lattitude = 0;
      data.longitude = 0;
    }
  }
}

PLANPOINTSTATUS_T FindTargetPoint(POS_T param)
{
  double distance = GetDistanceUTMFromGPS(param.longitude, param.lattitude, g_points_plan[0].longitude, g_points_plan[0].lattitude);
  ROS_INFO("distance is %s, g_points_plan.size is %d", DoubleToString(distance, 8).c_str(), g_points_plan.size());

  while (distance < preview_point && 0 != g_points_plan.size())
  {
    std::vector<POS_T>::iterator k = g_points_plan.begin();
    g_points_plan.erase(k);

    distance = GetDistanceUTMFromGPS(param.longitude, param.lattitude, g_points_plan[0].longitude, g_points_plan[0].lattitude);
    ROS_INFO("distance is %s, g_points_plan.size is %d", DoubleToString(distance, 8).c_str(), g_points_plan.size());
  }

  if (distance < preview_point)
  {
    if (0 == g_points_plan.size())
    {
      return NoPoints;
    }
    else
    {
      return Errors;
    }    
  }
  else
  {
    if (0 == g_points_plan.size())
    {
      return Errors;
    }
    else
    {
      return Normal;
    }    
  }
}

void update_rviz_points()
{
  geometry_msgs::Point point;
  geometry_msgs::Point point_first;

  LonLat2UTM(g_points_plan[0].longitude, g_points_plan[0].lattitude, point_first.x, point_first.y);

  g_rviz_points_plan.points.clear();
  for (uint16_t i = 0; i < g_points_plan.size(); i++)
  {
    LonLat2UTM(g_points_plan[i].longitude, g_points_plan[i].lattitude, point.x, point.y);
    point.x -= point_first.x;
    point.y -= point_first.y;
    g_rviz_points_plan.points.push_back(point);
  }

  g_rviz_point_current.points.clear();
  for (uint16_t i = 0; i < g_points_current.size(); i++)
  {
    LonLat2UTM(g_points_current[i].longitude, g_points_current[i].lattitude, point.x, point.y);
    point.x -= point_first.x;
    point.y -= point_first.y;
    g_rviz_point_current.points.push_back(point);
  }
}

void init_rviz_points()
{
  g_rviz_points_plan.header.frame_id = "/my_frame";
  g_rviz_points_plan.header.stamp = ros::Time::now();
  g_rviz_points_plan.ns = "points_and_lines";
  g_rviz_points_plan.action = visualization_msgs::Marker::ADD;
  g_rviz_points_plan.pose.orientation.w = 1.0;

  g_rviz_points_plan.id = 0;

  g_rviz_points_plan.type = visualization_msgs::Marker::POINTS;

  g_rviz_points_plan.scale.x = 0.2;
  g_rviz_points_plan.scale.y = 0.2;

  g_rviz_points_plan.color.g = 1.0f;
  g_rviz_points_plan.color.a = 1.0;

  g_rviz_point_current.header.frame_id = "/my_frame";
  g_rviz_point_current.header.stamp = ros::Time::now();
  g_rviz_point_current.ns = "points_and_lines";
  g_rviz_point_current.action = visualization_msgs::Marker::ADD;
  g_rviz_point_current.pose.orientation.w = 1.0;

  g_rviz_point_current.id = 1;

  g_rviz_point_current.type = visualization_msgs::Marker::POINTS;

  g_rviz_point_current.scale.x = 1.0;
  g_rviz_point_current.scale.y = 1.0;

  g_rviz_point_current.color.a = 1.0;
  g_rviz_point_current.color.r = 1.0f;
}